#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/pgmspace.h> 
#include <stdio.h>
#include <util/delay.h>

#include "lib/Hd44780/CharDisplay.h"
#include "lib/Hd44780/Hd44780_Direct.h"
#include "lib/Button/Buttons.h"
#include "lib/pwm/pwm.h"

#define MODE_FAST_SQUARE	0x00
#define MODE_SLOW_SQUARE	0x01
#define MODE_SQUARE			0x02
#define MODE_SINE			0x03
#define MODE_TRIANGLE		0x04
#define MODE_SAWTOOTH_UP	0x05
#define MODE_SAWTOOTH_DOWN	0x06
#define MODE_STAIRCASE_UP	0x07
#define MODE_STAIRCASE_DOWN	0x08
#define MODE_SERVO			0x09
#define MODE_VOLTAGE		0x0A
#define MODE_FIRST			MODE_FAST_SQUARE
#define MODE_LAST 			MODE_VOLTAGE

#define DDS_MAX				495
#define SERVO_MAX			1100

#define BUTTON_MODE			_BV(PORTC1)
#define BUTTON_UP			_BV(PORTC2)
#define BUTTON_DOWN			_BV(PORTC3)

using namespace digitalcave;

const uint8_t data_square[] PROGMEM			=	{0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
const uint8_t data_sine[] PROGMEM			=	{0x7f,0x82,0x85,0x88,0x8b,0x8e,0x91,0x94,0x97,0x9b,0x9e,0xa1,0xa4,0xa7,0xaa,0xad,0xaf,0xb2,0xb5,0xb8,0xbb,0xbe,0xc0,0xc3,0xc6,0xc8,0xcb,0xcd,0xd0,0xd2,0xd4,0xd7,0xd9,0xdb,0xdd,0xdf,0xe1,0xe3,0xe5,0xe7,0xe9,0xeb,0xec,0xee,0xef,0xf1,0xf2,0xf4,0xf5,0xf6,0xf7,0xf8,0xf9,0xfa,0xfb,0xfb,0xfc,0xfd,0xfd,0xfe,0xfe,0xfe,0xfe,0xfe,0xff,0xfe,0xfe,0xfe,0xfe,0xfe,0xfd,0xfd,0xfc,0xfb,0xfb,0xfa,0xf9,0xf8,0xf7,0xf6,0xf5,0xf4,0xf2,0xf1,0xef,0xee,0xec,0xeb,0xe9,0xe7,0xe5,0xe3,0xe1,0xdf,0xdd,0xdb,0xd9,0xd7,0xd4,0xd2,0xd0,0xcd,0xcb,0xc8,0xc6,0xc3,0xc0,0xbe,0xbb,0xb8,0xb5,0xb2,0xaf,0xad,0xaa,0xa7,0xa4,0xa1,0x9e,0x9b,0x97,0x94,0x91,0x8e,0x8b,0x88,0x85,0x82,0x7f,0x7c,0x79,0x76,0x73,0x70,0x6d,0x6a,0x67,0x63,0x60,0x5d,0x5a,0x57,0x54,0x51,0x4f,0x4c,0x49,0x46,0x43,0x40,0x3e,0x3b,0x38,0x36,0x33,0x31,0x2e,0x2c,0x2a,0x27,0x25,0x23,0x21,0x1f,0x1d,0x1b,0x19,0x17,0x15,0x13,0x12,0x10,0x0f,0x0d,0x0c,0x0a,0x09,0x08,0x07,0x06,0x05,0x04,0x03,0x03,0x02,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x02,0x03,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0c,0x0d,0x0f,0x10,0x12,0x13,0x15,0x17,0x19,0x1b,0x1d,0x1f,0x21,0x23,0x25,0x27,0x2a,0x2c,0x2e,0x31,0x33,0x36,0x38,0x3b,0x3e,0x40,0x43,0x46,0x49,0x4c,0x4f,0x51,0x54,0x57,0x5a,0x5d,0x60,0x63,0x67,0x6a,0x6d,0x70,0x73,0x76,0x79,0x7c};
const uint8_t data_triangle[] PROGMEM		=	{0x00,0x02,0x04,0x06,0x08,0x0a,0x0c,0x0e,0x10,0x12,0x14,0x16,0x18,0x1a,0x1c,0x1e,0x20,0x22,0x24,0x26,0x28,0x2a,0x2c,0x2e,0x30,0x32,0x34,0x36,0x38,0x3a,0x3c,0x3e,0x40,0x42,0x44,0x46,0x48,0x4a,0x4c,0x4e,0x50,0x52,0x54,0x56,0x58,0x5a,0x5c,0x5e,0x60,0x62,0x64,0x66,0x68,0x6a,0x6c,0x6e,0x70,0x72,0x74,0x76,0x78,0x7a,0x7c,0x7e,0x80,0x82,0x84,0x86,0x88,0x8a,0x8c,0x8e,0x90,0x92,0x94,0x96,0x98,0x9a,0x9c,0x9e,0xa0,0xa2,0xa4,0xa6,0xa8,0xaa,0xac,0xae,0xb0,0xb2,0xb4,0xb6,0xb8,0xba,0xbc,0xbe,0xc0,0xc2,0xc4,0xc6,0xc8,0xca,0xcc,0xce,0xd0,0xd2,0xd4,0xd6,0xd8,0xda,0xdc,0xde,0xe0,0xe2,0xe4,0xe6,0xe8,0xea,0xec,0xee,0xf0,0xf2,0xf4,0xf6,0xf8,0xfa,0xfc,0xfe,0xff,0xfe,0xfc,0xfa,0xf8,0xf6,0xf4,0xf2,0xf0,0xee,0xec,0xea,0xe8,0xe6,0xe4,0xe2,0xe0,0xde,0xdc,0xda,0xd8,0xd6,0xd4,0xd2,0xd0,0xce,0xcc,0xca,0xc8,0xc6,0xc4,0xc2,0xc0,0xbe,0xbc,0xba,0xb8,0xb6,0xb4,0xb2,0xb0,0xae,0xac,0xaa,0xa8,0xa6,0xa4,0xa2,0xa0,0x9e,0x9c,0x9a,0x98,0x96,0x94,0x92,0x90,0x8e,0x8c,0x8a,0x88,0x86,0x84,0x82,0x80,0x7e,0x7c,0x7a,0x78,0x76,0x74,0x72,0x70,0x6e,0x6c,0x6a,0x68,0x66,0x64,0x62,0x60,0x5e,0x5c,0x5a,0x58,0x56,0x54,0x52,0x50,0x4e,0x4c,0x4a,0x48,0x46,0x44,0x42,0x40,0x3e,0x3c,0x3a,0x38,0x36,0x34,0x32,0x30,0x2e,0x2c,0x2a,0x28,0x26,0x24,0x22,0x20,0x1e,0x1c,0x1a,0x18,0x16,0x14,0x12,0x10,0x0e,0x0c,0x0a,0x08,0x06,0x04,0x02};
const uint8_t data_sawtooth_up[] PROGMEM	=	{0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,0x18,0x19,0x1a,0x1b,0x1c,0x1d,0x1e,0x1f,0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27,0x28,0x29,0x2a,0x2b,0x2c,0x2d,0x2e,0x2f,0x30,0x31,0x32,0x33,0x34,0x35,0x36,0x37,0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,0x40,0x41,0x42,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4a,0x4b,0x4c,0x4d,0x4e,0x4f,0x50,0x51,0x52,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5a,0x5b,0x5c,0x5d,0x5e,0x5f,0x60,0x61,0x62,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6a,0x6b,0x6c,0x6d,0x6e,0x6f,0x70,0x71,0x72,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7a,0x7b,0x7c,0x7d,0x7e,0x7f,0x80,0x81,0x82,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8a,0x8b,0x8c,0x8d,0x8e,0x8f,0x90,0x91,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9a,0x9b,0x9c,0x9d,0x9e,0x9f,0xa0,0xa1,0xa2,0xa3,0xa4,0xa5,0xa6,0xa7,0xa8,0xa9,0xaa,0xab,0xac,0xad,0xae,0xaf,0xb0,0xb1,0xb2,0xb3,0xb4,0xb5,0xb6,0xb7,0xb8,0xb9,0xba,0xbb,0xbc,0xbd,0xbe,0xbf,0xc0,0xc1,0xc2,0xc3,0xc4,0xc5,0xc6,0xc7,0xc8,0xc9,0xca,0xcb,0xcc,0xcd,0xce,0xcf,0xd0,0xd1,0xd2,0xd3,0xd4,0xd5,0xd6,0xd7,0xd8,0xd9,0xda,0xdb,0xdc,0xdd,0xde,0xdf,0xe0,0xe1,0xe2,0xe3,0xe4,0xe5,0xe6,0xe7,0xe8,0xe9,0xea,0xeb,0xec,0xed,0xee,0xef,0xf0,0xf1,0xf2,0xf3,0xf4,0xf5,0xf6,0xf7,0xf8,0xf9,0xfa,0xfb,0xfc,0xfd,0xfe,0xff};
const uint8_t data_sawtooth_down[] PROGMEM	=	{0xff,0xfe,0xfd,0xfc,0xfb,0xfa,0xf9,0xf8,0xf7,0xf6,0xf5,0xf4,0xf3,0xf2,0xf1,0xf0,0xef,0xee,0xed,0xec,0xeb,0xea,0xe9,0xe8,0xe7,0xe6,0xe5,0xe4,0xe3,0xe2,0xe1,0xe0,0xdf,0xde,0xdd,0xdc,0xdb,0xda,0xd9,0xd8,0xd7,0xd6,0xd5,0xd4,0xd3,0xd2,0xd1,0xd0,0xcf,0xce,0xcd,0xcc,0xcb,0xca,0xc9,0xc8,0xc7,0xc6,0xc5,0xc4,0xc3,0xc2,0xc1,0xc0,0xbf,0xbe,0xbd,0xbc,0xbb,0xba,0xb9,0xb8,0xb7,0xb6,0xb5,0xb4,0xb3,0xb2,0xb1,0xb0,0xaf,0xae,0xad,0xac,0xab,0xaa,0xa9,0xa8,0xa7,0xa6,0xa5,0xa4,0xa3,0xa2,0xa1,0xa0,0x9f,0x9e,0x9d,0x9c,0x9b,0x9a,0x99,0x98,0x97,0x96,0x95,0x94,0x93,0x92,0x91,0x90,0x8f,0x8e,0x8d,0x8c,0x8b,0x8a,0x89,0x88,0x87,0x86,0x85,0x84,0x83,0x82,0x81,0x80,0x7f,0x7e,0x7d,0x7c,0x7b,0x7a,0x79,0x78,0x77,0x76,0x75,0x74,0x73,0x72,0x71,0x70,0x6f,0x6e,0x6d,0x6c,0x6b,0x6a,0x69,0x68,0x67,0x66,0x65,0x64,0x63,0x62,0x61,0x60,0x5f,0x5e,0x5d,0x5c,0x5b,0x5a,0x59,0x58,0x57,0x56,0x55,0x54,0x53,0x52,0x51,0x50,0x4f,0x4e,0x4d,0x4c,0x4b,0x4a,0x49,0x48,0x47,0x46,0x45,0x44,0x43,0x42,0x41,0x40,0x3f,0x3e,0x3d,0x3c,0x3b,0x3a,0x39,0x38,0x37,0x36,0x35,0x34,0x33,0x32,0x31,0x30,0x2f,0x2e,0x2d,0x2c,0x2b,0x2a,0x29,0x28,0x27,0x26,0x25,0x24,0x23,0x22,0x21,0x20,0x1f,0x1e,0x1d,0x1c,0x1b,0x1a,0x19,0x18,0x17,0x16,0x15,0x14,0x13,0x12,0x11,0x10,0x0f,0x0e,0x0d,0x0c,0x0b,0x0a,0x09,0x08,0x07,0x06,0x05,0x04,0x03,0x02,0x01,0x00};
const uint8_t data_staircase_up[] PROGMEM	=	{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0};
const uint8_t data_staircase_down[] PROGMEM	=	{0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xe0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xc0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0xa0,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x60,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

static Hd44780_Direct hd44780(hd44780.FUNCTION_LINE_2 | hd44780.FUNCTION_SIZE_5x8, &PORTB, 0, &PORTB, 4, &PORTB, 5, &PORTC, 4, &PORTC, 5, &PORTC, 0);
static CharDisplay display(&hd44780, 2, 16);
static Buttons buttons(&PORTC, BUTTON_MODE | BUTTON_UP | BUTTON_DOWN, 3, 8, 70, 8);

static uint8_t ui_freq_square = 0xFF;
static uint16_t ui_freq_dds = DDS_MAX;
static uint16_t ui_freq_servo = SERVO_MAX / 2;
static uint8_t ui_voltage = 0x00;
static uint8_t ui_voltage_running = 0x00;

volatile uint8_t _mode = MODE_FIRST;

uint8_t _data[256] __attribute__ ((aligned (256)));	//Copy PROGMEM DDS signals into this buffer.

uint32_t get_fast_square_frequency(){
	//From the datasheet, in the section detailing the output frequency of CTC _mode PWM
	return F_CPU / (2 * (1 + (uint32_t) (255 - ui_freq_square)));
}

uint32_t get_slow_square_frequency(){
	//From the datasheet, in the section detailing the output frequency of CTC _mode PWM
	return F_CPU / (2 * 256 * (1 + (uint32_t) (255 - ui_freq_square)));
}

uint16_t get_dds_frequency(){
	//Increment from 10Hz to 1.000kHz in 2Hz increments
	return (double) ((uint16_t) ui_freq_dds * 2) + 10;
}

uint32_t get_servo_phase(){
	//Return a value between 400us and 2600us with an increment of 1us.
	return ((ui_freq_servo * 2) + 400);
}

double get_voltage_full_range(){
	//We need to calibrate this to our specific hardware by adjusting the
	// multiplier and the offset.  The multiplier is nominally 10; this
	// should be set to the actual voltage range from your DAC between 
	// 0x00 and 0xFF.  The offset is the negative voltage.  Measure the
	// actual output voltage at value 0x00 and 0xFF and recompile for
	// these values.
	return 10.28 * (ui_voltage / 255.0) - 5.19;
}

double get_voltage_half_range(){
	//We need to calibrate this to our specific hardware by adjusting the
	// multiplier and the offset.  The multiplier is nominally 10; this
	// should be set to the actual voltage range from your DAC between 
	// 0x00 and 0xFF.  The offset is the negative voltage.  Measure the
	// actual output voltage at value 0x00 and 0xFF and recompile for
	// these values.
	return 5.15 * (ui_voltage / 255.0) - 0.048;
}

uint16_t _pwm_micros_to_clicks(uint32_t micros){
	return (((F_CPU / 1000000) * micros) / 8);
}

void update_display(){
	display.clear();
	_delay_ms(1);
	if (_mode == MODE_FAST_SQUARE){
		display.write_text(0, 0, "Square (Fast)", 13);
	}
	else if (_mode == MODE_SLOW_SQUARE){
		display.write_text(0, 0, "Square (Slow)", 13);
	}
	else if (_mode == MODE_SQUARE){
		display.write_text(0, 0, "Square (Analog)", 15);
	}
	else if (_mode == MODE_SINE){
		display.write_text(0, 0, "Sine", 4);
	}
	else if (_mode == MODE_TRIANGLE){
		display.write_text(0, 0, "Triangle", 8);
	}
	else if (_mode == MODE_SAWTOOTH_UP){
		display.write_text(0, 0, "Sawtooth Up", 11);
	}
	else if (_mode == MODE_SAWTOOTH_DOWN){
		display.write_text(0, 0, "Sawtooth Dn", 11);
	}
	else if (_mode == MODE_STAIRCASE_UP){
		display.write_text(0, 0, "Staircase Up", 12);
	}
	else if (_mode == MODE_STAIRCASE_DOWN){
		display.write_text(0, 0, "Staircase Dn", 12);
	}
	else if (_mode == MODE_SERVO){
		display.write_text(0, 0, "Servo", 5);
	}
	else if (_mode == MODE_VOLTAGE){
		display.write_text(0, 0, "Voltage Output", 14);
	}
	else {
		display.write_text(0, 0, "Unknown", 7);
	}
	
	if (_mode == MODE_SERVO){
		//For servo testing we care about phase, not frequency.  The period is fixed
		// at 20ms (20000us); the phase of the high section will determine the servo's 
		// output angle.  By spec, servos are supposed to listen for values between
		// 1ms and 2ms; however, most servos have some wiggle room and let you set a value
		// from about 0.5ms to 2.5ms.  For this servo test _mode, we support output phases 
		// in this expanded range, in 8us increments.  See get_servo_phase() for exact 
		// implementation details and range.
		uint32_t phase = get_servo_phase();
		char temp[16];
		uint8_t l;
		l = snprintf(temp, 16, "%6d us", (uint16_t) phase);
		_delay_us(100);
		display.write_text(1, 0, temp, l);
	}
	else if (_mode == MODE_VOLTAGE){
		double voltage_full = get_voltage_full_range();
		double voltage_half = get_voltage_half_range();
		char temp[16];
		uint8_t l;
		l = snprintf(temp, 16, "%+4.2fV / %+4.2fV", voltage_full, voltage_half);
		_delay_us(100);
		display.write_text(1, 0, temp, l);
	}
	else{
		uint32_t frequency;
		if (_mode == MODE_FAST_SQUARE) frequency = get_fast_square_frequency();
		else if (_mode == MODE_SLOW_SQUARE) frequency = get_slow_square_frequency();
		else frequency = get_dds_frequency();
	
		char temp[16];
		uint8_t l;
		if (frequency >= 1000000){
			l = snprintf(temp, 16, "%6.4f MHz", (frequency / 1000000.0));
		}
		else if (frequency >= 1000){
			l = snprintf(temp, 16, " %5.3f kHz", (frequency / 1000.0));
		}
		else {
			l = snprintf(temp, 16, "    %3d Hz", (uint16_t) frequency);
		}
		_delay_us(100);
		display.write_text(1, 0, temp, l);
	}
	
	if (TCCR0B != 0x00 || TCCR1B != 0x00 || ui_voltage_running != 0x00){
		_delay_us(100);
		display.write_text(0, 0x0F, "#", 1);
	}
	
	_delay_ms(1);
}

void update_square_frequency(){
	OCR0A = (255 - ui_freq_square);					//Comparator
}

void update_servo_phase(){
	OCR1B = _pwm_micros_to_clicks(get_servo_phase()); //OCR1B controls the PWM phase
}

void update_dds_frequency(){
	uint32_t frequency = get_dds_frequency();
	OCR1A = (F_CPU / 256 / frequency) - 1;			//Comparison value
}

void update_voltage(){
	PORTD = ui_voltage;
}

void main_menu(){
	update_display();
	
	while(1){
		_delay_ms(12);
		buttons.sample();

		uint8_t pressed = buttons.pressed();
		uint8_t released = buttons.released();
		uint8_t held = buttons.held();
		uint8_t repeat = buttons.repeat();

		if (released & BUTTON_MODE){
			_mode++;
			if (_mode > MODE_LAST){
				_mode = MODE_FIRST;
			}
			update_display();
		}
		else if (held & BUTTON_MODE){
			update_display();
			return;
		}
		
		if (_mode == MODE_FAST_SQUARE || _mode == MODE_SLOW_SQUARE){
			if (pressed & BUTTON_UP){
				ui_freq_square++;
				update_display();
			}
			else if (pressed & BUTTON_DOWN){
				ui_freq_square--;
				update_display();
			}
			else if (repeat & BUTTON_UP){
				ui_freq_square += 4;
				update_display();
			}
			else if (repeat & BUTTON_DOWN){
				ui_freq_square -= 4;
				update_display();
			}
		}
		else if (_mode == MODE_SERVO){
			if (pressed & BUTTON_UP){
				ui_freq_servo++;
				if (ui_freq_servo > SERVO_MAX) ui_freq_servo = 0;
				update_display();
			}
			else if (pressed & BUTTON_DOWN){
				ui_freq_servo--;
				if (ui_freq_servo > SERVO_MAX) ui_freq_servo = SERVO_MAX;
				update_display();
			}
			else if (repeat & BUTTON_UP){
				ui_freq_servo += 4;
				if (ui_freq_servo > SERVO_MAX) ui_freq_servo = 0;
				update_display();
			}
			else if (repeat & BUTTON_DOWN){
				ui_freq_servo -= 4;
				if (ui_freq_servo > SERVO_MAX) ui_freq_servo = SERVO_MAX;
				update_display();
			}
		}
		else if (_mode == MODE_VOLTAGE){
			if (pressed & BUTTON_UP){
				ui_voltage++;
				update_display();
			}
			else if (pressed & BUTTON_DOWN){
				ui_voltage--;
				update_display();
			}
			else if (repeat & BUTTON_UP){
				ui_voltage += 4;
				update_display();
			}
			else if (repeat & BUTTON_DOWN){
				ui_voltage -= 4;
				update_display();
			}
		}
		else {
			if (pressed & BUTTON_UP){
				ui_freq_dds++;
				if (ui_freq_dds > DDS_MAX) ui_freq_dds = 0;
				update_display();
			}
			else if (pressed & BUTTON_DOWN){
				ui_freq_dds--;
				if (ui_freq_dds > DDS_MAX) ui_freq_dds = DDS_MAX;
				update_display();
			}
			else if (repeat & BUTTON_UP){
				ui_freq_dds += 10;
				if (ui_freq_dds > DDS_MAX) ui_freq_dds = 0;
				update_display();
			}
			else if (repeat & BUTTON_DOWN){
				ui_freq_dds -= 10;
				if (ui_freq_dds > DDS_MAX) ui_freq_dds = DDS_MAX;
				update_display();
			}
		}
	}
}

void square_menu(){
	update_display();

	while(1){	//Timers do everything now.
		_delay_ms(12);
		buttons.sample();
		
		uint8_t pressed = buttons.pressed();
		uint8_t held = buttons.held();
		uint8_t repeat = buttons.repeat();

		if (held & BUTTON_MODE){
			TCCR0B = 0x00;
			TIMSK0 = 0x00;
			update_display();
			return;
		}
		else if (pressed & BUTTON_UP){
			ui_freq_square++;
			update_display();
			update_square_frequency();
		}
		else if (pressed & BUTTON_DOWN){
			ui_freq_square--;
			update_display();
			update_square_frequency();
		}
		else if (repeat & BUTTON_UP){
			ui_freq_square += 4;
			update_display();
			update_square_frequency();
		}
		else if (repeat & BUTTON_DOWN){
			ui_freq_square -= 4;
			update_display();
			update_square_frequency();
		}
	}
}

void dds_menu(){
	update_display();

	while(1){	//Timers do everything now.
		_delay_ms(12);
		buttons.sample();
		
		uint8_t pressed = buttons.pressed();
		uint8_t held = buttons.held();
		uint8_t repeat = buttons.repeat();

		if (held & BUTTON_MODE){
			TCCR1B = 0x00;
			TIMSK1 = 0x00;
			update_display();
			return;
		}
		else if (pressed & BUTTON_UP){
			ui_freq_dds++;
			if (ui_freq_dds > DDS_MAX) ui_freq_dds = 0;
			update_display();
			update_dds_frequency();
		}
		else if (pressed & BUTTON_DOWN){
			ui_freq_dds--;
			if (ui_freq_dds > DDS_MAX) ui_freq_dds = DDS_MAX;
			update_display();
			update_dds_frequency();
		}
		else if (repeat & BUTTON_UP){
			ui_freq_dds += 10;
			if (ui_freq_dds > DDS_MAX) ui_freq_dds = 0;
			update_display();
			update_dds_frequency();
		}
		else if (repeat & BUTTON_DOWN){
			ui_freq_dds -= 10;
			if (ui_freq_dds > DDS_MAX) ui_freq_dds =DDS_MAX;
			update_display();
			update_dds_frequency();
		}
	}
}

void servo_menu(){
	update_display();

	while(1){
		_delay_ms(12);
		buttons.sample();
		
		uint8_t pressed = buttons.pressed();
		uint8_t held = buttons.held();
		uint8_t repeat = buttons.repeat();

		if (held & BUTTON_MODE){
			TCCR1B = 0x00;
			PORTC &= ~_BV(PORTC5);
			update_display();
			return;
		}
		else if (pressed & BUTTON_UP){
			ui_freq_servo++;
			if (ui_freq_servo > SERVO_MAX) ui_freq_servo = 0;
			update_display();
			update_servo_phase();
		}
		else if (pressed & BUTTON_DOWN){
			ui_freq_servo--;
			if (ui_freq_servo > SERVO_MAX) ui_freq_servo = SERVO_MAX;
			update_display();
			update_servo_phase();
		}
		else if (repeat & BUTTON_UP){
			ui_freq_servo += 4;
			if (ui_freq_servo > SERVO_MAX) ui_freq_servo = 0;
			update_display();
			update_servo_phase();
		}
		else if (repeat & BUTTON_DOWN){
			ui_freq_servo -= 4;
			if (ui_freq_servo > SERVO_MAX) ui_freq_servo = SERVO_MAX;
			update_display();
			update_servo_phase();
		}
	}
}

void voltage_menu(){
	ui_voltage_running = 0x01;
	update_display();

	while(1){
		_delay_ms(12);
		buttons.sample();
		
		uint8_t pressed = buttons.pressed();
		uint8_t held = buttons.held();
		uint8_t repeat = buttons.repeat();

		if (held & BUTTON_MODE){
			PORTD = 0x00;
			ui_voltage_running = 0x00;
			update_display();
			return;
		}
		else if (pressed & BUTTON_UP){
			ui_voltage++;
			update_voltage();
			update_display();
		}
		else if (pressed & BUTTON_DOWN){
			ui_voltage--;
			update_voltage();
			update_display();
		}
		else if (repeat & BUTTON_UP){
			ui_voltage += 4;
			update_voltage();
			update_display();
		}
		else if (repeat & BUTTON_DOWN){
			ui_voltage -= 4;
			update_voltage();
			update_display();
		}
	}
}

void output_waveform(){
	if (_mode == MODE_FAST_SQUARE || _mode == MODE_SLOW_SQUARE){
		TCCR0A = _BV(COM0A0) | _BV(WGM01);			//CTC Mode (mode 2), Toggle OC0A on compare match
		
		if (_mode == MODE_FAST_SQUARE) TCCR0B = _BV(CS00);					//No prescaler
		else TCCR0B = _BV(CS02);											//Div 256 prescaler
		update_square_frequency();
		TCNT0 = 0;
		sei();
		
		square_menu();
	}
	else if (_mode == MODE_SERVO){
		OCR1A = _pwm_micros_to_clicks(20000); //OCR1A controls the PWM period
		update_servo_phase();
		
		//Enable compare interrupt on both channels
		TIMSK1 = _BV(OCIE1A) | _BV(OCIE1B);

		TCNT1 = 0x00;
		TCCR1A = 0x00;
		TCCR1B |= _BV(CS11);		//Div 8 prescaler
		sei();
		
		servo_menu();
	}
	else if (_mode == MODE_VOLTAGE){
		update_voltage();
		voltage_menu();
	}
	else {
		const uint8_t* progmem_pointer;
		
		//Figure out which DDS mode we are in
		if (_mode == MODE_SQUARE) progmem_pointer = data_square;
		else if (_mode == MODE_SINE) progmem_pointer = data_sine;
		else if (_mode == MODE_TRIANGLE) progmem_pointer = data_triangle;
		else if (_mode == MODE_SAWTOOTH_UP) progmem_pointer = data_sawtooth_up;
		else if (_mode == MODE_SAWTOOTH_DOWN) progmem_pointer = data_sawtooth_down;
		else if (_mode == MODE_STAIRCASE_UP) progmem_pointer = data_staircase_up;
		else if (_mode == MODE_STAIRCASE_DOWN) progmem_pointer = data_staircase_down;
		else return;
	
		//Copy from PROGMEM to RAM buffer
		for (uint16_t i = 0; i <= 0xFF; i++){
			_data[i] = pgm_read_byte_near(progmem_pointer + i);
		}
		
		TIMSK1 = _BV(TOIE1);								//Enable timer1 overflow interrupts
		TCNT1 = 0;
		sei();
		update_dds_frequency();
		TCCR1A = _BV(WGM11) | _BV(WGM10);					//Fast PWM mode 15
		TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);		//No prescaler

		dds_menu();
	}
}

int main (void){
	//Init port D and PORTC5 in output mode (used to send data to DAC and to servo, respectively)
	DDRD = 0xFF;
 	DDRC |= _BV(PORTC5);

	//Infinite loop of picking menu options, then outputting waveform.
	// Hold the mode button to stop / start signal generation.
	while (1) {
		//Pick mode / options
		main_menu();

		//Output waveform
		output_waveform();
	}
}

/* 
 * The frequency comparison.  When it overflows, we reset the timer to 0.
 */
/*
All of these are now implemented in assembly.  See timer1.S for details.
ISR(TIMER1_OVF_vect){
	PORTD = _data[_data_ptr];
	_data_ptr += _dds_increment;
}
ISR(TIMER1_COMPA_vect){	
	//Servo mode
	TCNT1 = 0;
	PORTD |= _BV(PORTD5);	
}
ISR(TIMER1_COMPB_vect){
	PORTD &= ~_BV(PORTD5);
}
*/
